﻿using System;
using System.Collections.Generic;
using TinyROS.Messages;
using System.Text.Json;
using System.Threading.Tasks;
using System.Net;
using System.Net.Http;
using System.Runtime.InteropServices;
using System.Drawing;
using System.Net.Sockets;
using System.IO;
using System.CodeDom.Compiler;
using Microsoft.CSharp;
using System.Reflection;


namespace TinyROS
{
    
    class Program
    {
        
        static void Main(string[] args)
        {
            TinyROSConfig.SaveConfig("LastRun.json");

            SystemCore core = new();
            //core.ConsoleSpin();

            using DiffCarNode car = new();
            DiffcarCmdMuxByTimeNode mux = new();
            MapClientNode mapClient = new();
            using ApplicationNode appServer = new();
            PlannerNode planner = new();

            var velIn1 = new DiffcarCourseTopic();
            var velIn2 = new DiffcarCourseTopic();
            var velIn3 = new DiffcarCourseTopic();

            core.RegisterTopic(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn1, velIn1);
            core.RegisterTopic(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn2, velIn2);
            core.RegisterTopic(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn3, velIn3);

            mux.AddInput(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn1, 1);
            mux.AddInput(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn2, 2);
            mux.AddInput(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn3, 3);

            ConstDiffcarSpeedNode zeroSpeed = new(TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn3);

            core.RegisterNode("testDiffcar", car);
            core.RegisterNode("mux", mux);
            core.RegisterNode("zeroV", zeroSpeed);
            core.RegisterNode("testPlanner", planner);
            core.RegisterNode("appServer", appServer);
            core.RegisterNode("mapClient", mapClient);

            core.RunNode("testDiffcar");
            core.RunNode("mux");
            core.RunNode("zeroV");
            core.RunNode("testPlanner");
            core.RunNode("mapClient");
            core.RunNode("appServer");

            core.ConsoleSpin();




        }
    }

    public static class TinyROSConfig
    {
        public static class DefaultTopicName
        {
            public static string DiffcarCommand = "cmd/vel";
            public static string DiffcarCommandMuxIn0 = "mux/cmd/vel/in0";
            public static string DiffcarCommandMuxIn1 = "mux/cmd/vel/in1";
            public static string DiffcarCommandMuxIn2 = "mux/cmd/vel/in2";
            public static string DiffcarCommandMuxIn3 = "mux/cmd/vel/in3";
            public static string DiffcarCommandMuxIn4 = "mux/cmd/vel/in4";
            public static string DiffcarCommandMuxIn5 = "mux/cmd/vel/in5";
            public static string DiffcarVelocity = "sensor/diffcar/motor";
            public static string DiffcarOrien = "sensor/diffcar/magnetometer";
            public static string CarStatus = "car/status";
            public static string EstimatedCarStaus = "car/estimatedStatus";
            public static string Map = "map";
            public static string Goal = "car/goal";
            public static string MapStream = "stream/map";
            public static class NodeCommand
            {
                public static string MapClient = "nodecmd/mapclient";
                public static string Planner = "nodecmd/planner";
            }

        }
        public static class Stm32
        {
            public static class Port
            {
                public static string Name { get; set; } = @"/dev/ttyAMA0";
            }
        }
        public static class RuntimeParam
        {
            public static class Map
            {
                [Obsolete]
                public static int Height { get; set; } = 100;
                [Obsolete]
                public static int Width { get; set; } = 100;
                public static int Size { get; set; } = 100;
                public static int ObstacleNum { get; set; } = 5;
                public static int ObstaclePercent { get; set; } = 50;
            }
        }
        public static class NetWork
        {
            public static class Protocol
            {
                public const string Http = "http://";
            }
            public static class MapServer
            {
                public static string BaseAddress { get; set; } = "192.168.43.139:8088";
                public static string GetMapURL { get; set; } = @"/map/get/0";
                public static string CreateMapURL { get; set; } = @"/map/create";
            }
            public static class AppServer
            {
                public static string Port = @":8089";
                public const string GetMapURL = @"/map/get/";
                public const string GetStatusURL = @"/car/status/get/";
                public const string GetServerStatusURL = @"/status/get/";
                public const string PostStringURL = @"/post/str/";
                public const string PostFloatURL = @"/post/float/";
                public const string PostEmptyURL = @"/post/empty/";
            }
        }
        public static void LoadConfig(string path)
        {
            var configStr = File.ReadAllText(path);
            JsonConfig config = JsonSerializer.Deserialize<JsonConfig>(configStr);
            TinyROSConfig.NetWork.MapServer.BaseAddress = config.MapServer.BaseAddress;
            TinyROSConfig.NetWork.MapServer.CreateMapURL = config.MapServer.CreateMapURL;
            TinyROSConfig.NetWork.MapServer.GetMapURL = config.MapServer.GetMapURL;
            TinyROSConfig.RuntimeParam.Map.Size = config.Map.Size;
            TinyROSConfig.RuntimeParam.Map.ObstacleNum = config.Map.ObstacleNum;
            TinyROSConfig.RuntimeParam.Map.ObstaclePercent = config.Map.ObstaclePercent;
            TinyROSConfig.NetWork.AppServer.Port = config.AppServer.Port;
            TinyROSConfig.Stm32.Port.Name = config.Stm32.PortName;
            TinyROSConfig.DefaultTopicName.CarStatus = config.DefaultTopicName.CarStatus;
            TinyROSConfig.DefaultTopicName.DiffcarCommand = config.DefaultTopicName.DiffcarCommand;
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn0 = config.DefaultTopicName.DiffcarCommandMuxIn0;
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn1 = config.DefaultTopicName.DiffcarCommandMuxIn1;
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn1 = config.DefaultTopicName.DiffcarCommandMuxIn2;
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn3 = config.DefaultTopicName.DiffcarCommandMuxIn3;
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn4 = config.DefaultTopicName.DiffcarCommandMuxIn4;
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn5 = config.DefaultTopicName.DiffcarCommandMuxIn5;
            TinyROSConfig.DefaultTopicName.DiffcarOrien = config.DefaultTopicName.DiffcarOrien;
            TinyROSConfig.DefaultTopicName.DiffcarVelocity = config.DefaultTopicName.DiffcarVelocity;
            TinyROSConfig.DefaultTopicName.EstimatedCarStaus = config.DefaultTopicName.EstimatedCarStaus;
            TinyROSConfig.DefaultTopicName.Goal = config.DefaultTopicName.Goal;
            TinyROSConfig.DefaultTopicName.Map = config.DefaultTopicName.Map;
            TinyROSConfig.DefaultTopicName.MapStream = config.DefaultTopicName.MapStream;
            TinyROSConfig.DefaultTopicName.NodeCommand.MapClient = config.DefaultTopicName.MapClient;
            TinyROSConfig.DefaultTopicName.NodeCommand.Planner = config.DefaultTopicName.Planner;
            //throw new NotImplementedException();
        }
        public static void SaveConfig(string path)
        {
            //var f = File.Create(path);
            JsonConfig config = new();
            config.MapServer = new();
            config.Map = new();
            config.DefaultTopicName = new();
            config.Stm32 = new();
            config.AppServer = new();
            config.MapServer.BaseAddress = TinyROSConfig.NetWork.MapServer.BaseAddress;
            config.MapServer.CreateMapURL = TinyROSConfig.NetWork.MapServer.CreateMapURL;
            config.MapServer.GetMapURL = TinyROSConfig.NetWork.MapServer.GetMapURL;
            config.Map.Size = TinyROSConfig.RuntimeParam.Map.Size;
            config.Map.ObstacleNum = TinyROSConfig.RuntimeParam.Map.ObstacleNum;
            config.Map.ObstaclePercent = TinyROSConfig.RuntimeParam.Map.ObstaclePercent;
            config.AppServer.Port = TinyROSConfig.NetWork.AppServer.Port;
            config.Stm32.PortName = TinyROSConfig.Stm32.Port.Name;
            config.DefaultTopicName.CarStatus = TinyROSConfig.DefaultTopicName.CarStatus;
            config.DefaultTopicName.DiffcarCommand = TinyROSConfig.DefaultTopicName.DiffcarCommand;
            config.DefaultTopicName.DiffcarCommandMuxIn0 = TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn0;
            config.DefaultTopicName.DiffcarCommandMuxIn1 = TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn1;
            config.DefaultTopicName.DiffcarCommandMuxIn2 = TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn2;
            config.DefaultTopicName.DiffcarCommandMuxIn3 = TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn3;
            config.DefaultTopicName.DiffcarCommandMuxIn4 = TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn4;
            config.DefaultTopicName.DiffcarCommandMuxIn5 = TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn5;
            config.DefaultTopicName.DiffcarOrien = TinyROSConfig.DefaultTopicName.DiffcarOrien;
            config.DefaultTopicName.DiffcarVelocity = TinyROSConfig.DefaultTopicName.DiffcarVelocity;
            config.DefaultTopicName.EstimatedCarStaus = TinyROSConfig.DefaultTopicName.EstimatedCarStaus;
            config.DefaultTopicName.Goal = TinyROSConfig.DefaultTopicName.Goal;
            config.DefaultTopicName.Map = TinyROSConfig.DefaultTopicName.Map;
            config.DefaultTopicName.MapStream = TinyROSConfig.DefaultTopicName.MapStream;
            config.DefaultTopicName.MapClient = TinyROSConfig.DefaultTopicName.NodeCommand.MapClient;
            config.DefaultTopicName.Planner = TinyROSConfig.DefaultTopicName.NodeCommand.Planner;
            string configStr = JsonSerializer.Serialize<JsonConfig>(config);
            File.WriteAllText(path, configStr);
        }
        private class JsonConfig
        {
            public JcfgMapServer MapServer { get; set; }
            public JcfgMap Map { get; set; }
            public JcfgAppServer AppServer { get; set; }
            public JcfgStm32 Stm32 { get; set; }
            public JcfgDefaultTopicName DefaultTopicName { get; set; }
        }
        private class JcfgMapServer
        {
            public string BaseAddress { get; set; }
            public string GetMapURL { get; set; }
            public string CreateMapURL { get; set; }
        }
        private class JcfgMap
        {
            public int Size { get; set; }
            public int ObstacleNum { get; set; }
            public int ObstaclePercent { get; set; }
        }
        private class JcfgStm32
        {
            public string PortName { get; set; }
        }
        private class JcfgAppServer
        {
            public string Port { get; set; }
        }
        private class JcfgDefaultTopicName
        {
            public string DiffcarCommand { get; set; }
            public string DiffcarCommandMuxIn0 { get; set; }
            public string DiffcarCommandMuxIn1 { get; set; }
            public string DiffcarCommandMuxIn2 { get; set; }
            public string DiffcarCommandMuxIn3 { get; set; }
            public string DiffcarCommandMuxIn4 { get; set; }
            public string DiffcarCommandMuxIn5 { get; set; }
            public string DiffcarVelocity { get; set; }
            public string DiffcarOrien { get; set; }
            public string CarStatus { get; set; }
            public string EstimatedCarStaus { get; set; }
            public string Map { get; set; }
            public string Goal { get; set; }
            public string MapStream { get; set; }
            public string MapClient { get; set; }
            public string Planner { get; set; }
        }

    }

}
